/* #include "algorithm_pid.h" */

#ifndef __ALGORITHM_PID_H
#define __ALGORITHM_PID_H

#include "stm32f4xx.h"
/*====================================================================================================*/
/*====================================================================================================*/
typedef struct {
  fp32 Kp;
  fp32 Ki;
  fp32 Kd;
  fp32 Err0;
  fp32 Err1;
  fp32 Err2;
  fp32 SumErr;
  fp32 ZeroErr;
  fp32 Output;
  fp32 KiMax;
  fp32 KiMin;
  fp32 OutMax;
  fp32 OutMin;
} PID_ST;
/*====================================================================================================*/
/*====================================================================================================*/
void PID_Init( PID_ST *PID, fp32 Kp, fp32 Ki, fp32 Kd );
fp32 PID_IncCal( PID_ST *PID, fp32 CurrentVal );
fp32 PID_PosCal( PID_ST *PID, fp32 CurrentVal );
fp32 PID_AHRS_Cal( PID_ST *PID, fp32 Angle, fp32 Gyroscope );
fp32 PID_AHRS_CalYaw( PID_ST *PID, fp32 Angle, fp32 Gyroscope );
/*====================================================================================================*/
/*====================================================================================================*/
extern PID_ST PID_Yaw;
extern PID_ST PID_Roll;
extern PID_ST PID_Pitch;
/*====================================================================================================*/
/*====================================================================================================*/
#endif	 
